/** 
 * @main	Plik z definicjami regulatorow
 * @file    regulatory.h
 * @author  Grzegorz Sejnota
 *
 * @created on 1 marca 2009, 10:41
 * @version 0.0001
 */

#include "regulatory.h"
#include "generatory.h"
#include "wyjatki.h"
#include <sstream>
#include <iostream>
#include <deque>
#include <numeric>
#include <algorithm>
#include <cmath>

using namespace std;



//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ POCZATEK [definicji interfejsu Regulator] +++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//======= funkcja wirtualna dodajGenerator =====================================

/**
 * Dodaje generator do vectora generatorow wartosci zdanej. Wartosc zadana jest
 * obliczana jako suma wyjsc wszystkich generatorow w vectorze.
 *		@param generator - wskaznik do generatora, ktory ma zostac dodany
 */
void Regulator::dodajGenerator(Generator* generator) {
    if (generator) generatory.push_back(generator);
}

//======= funkcja wirtualna dodajGenerator =====================================

/**
 * Dodaje generator do vectora generatorow wartosci zdanej. Wartosc zadana jest
 * obliczana jako suma wyjsc wszystkich generatorow w vectorze.
 *		@param generator - wskaznik do generatora, ktory ma zostac dodany
 */
void Regulator::dodajGenerator(std::map< std::string, std::string > mapaKonfig) {
    /*
    if (mapaKonfig["[typ]="].rfind("regP") != string::npos) {
            generatory.push_back(new RegulatorP());
            if (!(generatory.back()->ustawParametry(mapaKonfig))) {
                    //cout << endl << "bladaaaaaaa!@@@" << endl;
                    delete generator.back();
                    generatory.pop_back();
            }
    }*/
}

//======= funkcja wirtualna usunGenerator ======================================

/**
 * Usuwa generator o podanym indeksie (numerowane od 0) z vectora generatorow 
 * wartosci zdanej.
 *		@param indeks - indeks generatora, ktory ma byc usuniety
 */
void Regulator::usunGenerator(int indeks) {
    if (indeks < generatory.size()) {
        delete generatory.at(indeks);
        generatory.erase(generatory.begin() + indeks);
    }
}

//======= funkcja wirtualna usunGenerator ======================================

/**
 * Usuwa generator o podanej nazwie z vectora generatorow wartosci zdanej.
 *		@param nazwa - unikalna nazwa generatora
 */
void Regulator::usunGenerator(std::string nazwa) {
   std::vector<Generator*>::iterator it = znajdzGenerator(nazwa);
   if(it != generatory.end()) {
      delete (*it);
      generatory.erase(znajdzGenerator(nazwa));
   }
}

std::vector<Generator*>::iterator Regulator::znajdzGenerator(std::string nazwa) {
   std::vector<Generator*>::iterator it = generatory.begin();
   std::vector<Generator*>::iterator it2 = generatory.end();
   for(; it != generatory.end(); ++it) {
      if(!(nazwa.compare((*it)->pobierzNazwe()))) {
	 it2 = it;
      }
   }
   return it2;
}

//======= funkcja wirtualna resetGeneratorow ===================================

/**
 * Resetuje generatory wartosci zadanej. Metoda protected - powinna byc uzywana
 * w przyslonietej metodzie zerujPamiec interfejsu ObiektDyskretny.
 */
void Regulator::resetGeneratorow() {
    for (int i = 0; i < generatory.size(); i++) {
        generatory.at(i)->reset();
    }
}

//======= funkcja zmierzWyjscie ================================================

/**
 * Funkcja probkujaca Regulator. Funkcja symuluje dzialanie czujnika pomiarowego
 * - pobiera wartosc wyjscia obiektu w kolejnej chwili czasu - przyszlej probce 
 * przy zalozeniu braku zmiany sterowania.
 *      @return przyszle wyjscie z obiektu
 */
double Regulator::zmierzWyjscie() {
    return 0;
}

//======= funkcja wirtualna zerujPamiec ========================================
/**
 * Zeruje pamiec regulatora - impelementacja funkcji czysto wirtualnej interfejsu
 * ObiektDyskretny. Wywoluje metode resetGeneratorow. W wypadku impelementacji 
 * regulatora pamietajacego poprzednie stany obiektu np. PID nalezy przyslonic
 * te metode wersja zapewniajaca czyszczenie pamieci regulatora.
 */
void Regulator::zerujPamiec() {
    resetGeneratorow();
}

std::vector<std::string> Regulator::pokazUstawioneGeneratory() {
   std::vector<std::string> wynik;
   std::vector<Generator*>::iterator itGen = generatory.begin();
   for(; itGen != generatory.end(); ++itGen) {
      wynik.push_back((*itGen)->pobierzNazwe());
   }
   return wynik;
}

std::map<std::string,std::string> Regulator::pokazParametryGeneratora(std::string nazwa) {
   std::map<std::string,std::string> wynik; 
   std::vector<Generator*>::iterator it2 = znajdzGenerator(nazwa);  
   if(it2 != generatory.end()){
      wynik = (*it2)->pobierzParam();
   }   

   return wynik;
}

void Regulator::zmienParametryGeneratora(std::string nazwa, std::map<std::string,std::string> konfig) {
   std::vector<Generator*>::iterator it2 = znajdzGenerator(nazwa);
   if(it2 != generatory.end()) {
      (*it2)->ustawParam(konfig);
   }
}

//------------------------------------------------------------------------------
//-------------- KONIEC [definicji interfejsu Regulator] -----------------------
//------------------------------------------------------------------------------


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ POCZATEK [definicji klasy RegulatorP] +++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//======= definicja skladnika statycznego liczbaRegulatorowP ===================
/**
 * Skladnik statyczny do przechowywania aktualnej liczby obiektow klasy
 * RegulatorP.
 */
int RegulatorP::liczbaRegulatorowP = 0;

//======= definicja skladnika statycznego kluczeMapy ===========================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorP::kluczeMapy = inicjalizujKlucze();


//======= funkcja inicjalizujKlucze ============================================

/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorP::inicjalizujKlucze() {
    vector<string> wzor;
    wzor.push_back("[nazwa]=");
    wzor.push_back("[k]=");
    wzor.push_back("[typ]=");
    return wzor;
}


//======= konstruktor  =========================================================
/**
 * Konstruktor klasy RegulatorP.
 *      @param wzm - wzmocnienie regulatora
 */
RegulatorP::RegulatorP(double wzm) {
    this->wzmocnienie = wzm;
    this->sterowanie = 0.0;
    this->uchyb = 0.0;
    this->wartoscRegulowana = 0.0;
    this->wartoscZadana = 0.0;
    liczbaRegulatorowP++;
    stringstream strumien;
    strumien << liczbaRegulatorowP;
    this->nazwa = "regulatorP" + strumien.str();
}


//======= konstruktor kopiujacy ================================================
/**
 * Konstruktor kopiujacy klasy RegulatorP.
 *      @param &wzor - referencja do obiektu klasy RegulatorP, 
 *                                        kt�ry ma by� skopiowany.
 */
RegulatorP::RegulatorP(const RegulatorP & wzor) {
    this->wzmocnienie = wzor.wzmocnienie;
    this->generatory = wzor.generatory;
    this->sterowanie = wzor.sterowanie;
    this->uchyb = wzor.uchyb;
    this->wartoscRegulowana = wzor.wartoscRegulowana;
    this->wartoscZadana = wzor.wartoscZadana;
    liczbaRegulatorowP++;
    stringstream strumien;
    strumien << liczbaRegulatorowP;
    this->nazwa = "regulatorP" + strumien.str();
}


//======= destruktor  ==========================================================
/**
 * Destruktor klasy RegulatorP - aktualizuje liczbe obiektow RegulatorP.
 */
RegulatorP::~RegulatorP() {
    liczbaRegulatorowP--;
}


//======= funkcja symuluj ======================================================
/**
 * Symuluje dzialanie regulatora - generuje wartosc sterowania na podstawie 
 * otrzymanej wartosci wyjscia obiektu oraz uaktualnia wartosci uchybu, 
 * sterowania, wartosci zadanej oraz wartosci regulowanej. Funkcja jest implementacja
 * funkcji abstrakcyjnej interfejsu ObiektDyskretny
 *		@param y - wyjscie z obiektu regulacji
 *		@return sterowanie
 */
double RegulatorP::symuluj(double y) {
    wartoscRegulowana = y;
    // ---- generacja wartosciZadanej ------------------------------------------
    wartoscZadana = 0.0;
    for (int i = 0; i < generatory.size(); i++) {
        wartoscZadana += generatory.at(i)->generuj();
    }
    // ---- obliczenie uchybu --------------------------------------------------
    uchyb = wartoscZadana - wartoscRegulowana;
    // ---- obliczenie sterowania ----------------------------------------------
    sterowanie = wzmocnienie*uchyb;
    return sterowanie;
}

//======= funkcja zmierzWyjscie ================================================
/**
 * Funkcja probkujaca Regulator. Funkcja symuluje dzialanie czujnika pomiarowego
 * - pobiera wartosc wyjscia obiektu w kolejnej chwili czasu - przyszlej probce 
 * przy zalozeniu braku zmiany sterowania.
 *      @return przyszle wyjscie z obiektu
 */
double RegulatorP::zmierzWyjscie() {
    return sterowanie;
}


//======= funkcja ustawParametry  =============================================
/**
 * Ustawia parametry obiektu na podstawie otrzymanej mapy. Metoda wykorzystywana
 * podczas wczytywania obiektow z pliku konfiguracyjnego. Zwracana wartosc zalezy
 * od poprawnosci mapy podanej jako argument
 *		@param mapaKonfig - mapa z konfiguracja
 *		@return <code>true</code> jezeli mapa byla poprawna mapa RegulatoraP
 *				i udalo sie ustawic parametry
 *	            <code>false</code> w przeciwnym wypadku
 */
bool RegulatorP::ustawParametry(std::map<std::string, std::string> mapaKonfig) {
    stringstream strumien;
    strumien.exceptions(stringstream::badbit | stringstream::failbit);
    double tymczK;
    string tymczNazwa;
    
    //cout<<"wywolano ustawParametry P"<<endl;

    //----- sprawdzenie wielkosci mapy -----------------------------------------
    if (mapaKonfig.size() != RegulatorP::kluczeMapy.size())
        throw Wyjatek("Blad w funkcji RegulatorP::ustawParametry w obiekcie: " + this->pobierzNazwe()
            + ".\n Nie zmieniono parametrow!");

    //----- sprawdzenie poprawnosci kluczy mapy --------------------------------
    for (int i = 0; i < RegulatorP::kluczeMapy.size(); i++) {
        if (mapaKonfig[RegulatorP::kluczeMapy.at(i)].empty())
            throw Wyjatek("Blad w funkcji RegulatorP::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!"
                + "\n Brak klucza:" + RegulatorP::kluczeMapy.at(i));
    }

    for (map<string, string>::iterator it = mapaKonfig.begin(); it != mapaKonfig.end(); it++) {
        strumien.clear();
        strumien.str(it->second); //wpisanie do strumienia wartosci mapy
        try {
            if ((it->first) == ("[nazwa]=")) strumien >> tymczNazwa;
            else if ((it->first) == ("[k]=")) strumien >> tymczK;
        } catch (stringstream::failure e) { //wyjatek strumienia - nie wczytano poprawnie parametrow
            strumien.clear();
            throw Wyjatek("Blad w funkcji RegulatorP::ustawParametry w obiekcie: " + this->pobierzNazwe()
                    + ".\n Nie zmieniono parametrow!");
            return false;
        }
    }
    this->wzmocnienie = tymczK;
    this->nazwa = tymczNazwa;
    //cout<<endl<<"wczytanoooo"<<endl;
    return true;
}

//======= funkcja pobierzParametry  =============================================

/**
 * Pobiera parametry RegulatoraP. Parametry sa zwracane w mapie zgodnej z zapisem
 * pliku konfiguracyjnego
 *		@return mapa z konfiguracja
 */
map<string, string> RegulatorP::pobierzParametry() const {
    map<string, string> mapaKonfig;
    stringstream strumien;
    mapaKonfig["[typ]="] = "regulatorP";
    mapaKonfig["[nazwa]="] = nazwa;
    strumien << wzmocnienie;
    mapaKonfig["[k]="] = strumien.str();
    return mapaKonfig;
}

//======= funkcja wirtualna zerujPamiec ========================================
/**
 * Zeruje pamiec regulatora - impelementacja funkcji czysto wirtualnej interfejsu
 * ObiektDyskretny. Wywoluje metode resetGeneratorow. W wypadku impelementacji 
 * regulatora pamietajacego poprzednie stany obiektu np. PID nalezy przyslonic
 * te metode wersja zapewniajaca czyszczenie pamieci regulatora.
 */
void RegulatorP::zerujPamiec() {
	sterowanie = 0.0;
    uchyb = 0.0;
    wartoscRegulowana = 0.0;
    wartoscZadana = 0.0;
    resetGeneratorow();
}

//======= funkcja wypiszInfo ===================================================

/**
 * Funkcja wypisujaca informacje o regulatorze P. Wypisuje 
 * nazwe obiektu.
 */
void RegulatorP::wypiszInfo() const {
}

//------------------------------------------------------------------------------
//-------------- KONIEC [definicji klasy RegulatorP] ---------------------------
//------------------------------------------------------------------------------



//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ POCZATEK [definicji klasy RegulatorPID] +++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//======= definicja skladnika statycznego liczbaRegulatorowPID =================
/**
 * Skladnik statyczny do przechowywania aktualnej liczby obiektow klasy
 * RegulatorPID.
 */
int RegulatorPID::liczbaRegulatorowPID = 0;

//======= definicja skladnika statycznego kluczeMapy ===========================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorPID::kluczeMapy = inicjalizujKlucze();


//======= funkcja inicjalizujKlucze ============================================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorPID::inicjalizujKlucze() {
    vector<string> wzor;
    wzor.push_back("[nazwa]=");
    wzor.push_back("[k]=");
    wzor.push_back("[typ]=");
    wzor.push_back("[Ti]=");
    wzor.push_back("[Td]=");
    wzor.push_back("[N]=");
    wzor.push_back("[b]=");
    return wzor;
}


//======= konstruktor  =========================================================
/**
 * Konstruktor klasy RegulatorPID.
 *      @param k - wzmocnienie regulatora
 */
RegulatorPID::RegulatorPID(double k) {
    this->k = k;
    this->Td = 0;
    this->Ti = 100000;
    this->N = 1;
    this->czescI = 0.0;
    this->czescD = 0.0;
    this->sterowanie = 0.0;
    this->uchyb = 0.0;
    this->wartoscRegulowana = 0.0;
    this->wartoscZadana = 0.0;
    liczbaRegulatorowPID++;
    stringstream strumien;
    strumien << liczbaRegulatorowPID;
    this->nazwa = "RegulatorPID" + strumien.str();
}


//======= konstruktor kopiujacy ================================================
/**
 * Konstruktor kopiujacy klasy RegulatorPID.
 *      @param &wzor - referencja do obiektu klasy RegulatorPID, 
 *                      ktory ma byc skopiowany.
 */
RegulatorPID::RegulatorPID(const RegulatorPID & wzor) {
    this->k = wzor.k;
    this->Td = wzor.Td;
    this->Ti = wzor.Ti;
    this->N = wzor.N;
    this->czescI = wzor.czescI;
    this->czescD = wzor.czescD;
    this->generatory = wzor.generatory;
    this->sterowanie = wzor.sterowanie;
    this->uchyb = wzor.uchyb;
    this->wartoscRegulowana = wzor.wartoscRegulowana;
    this->wartoscZadana = wzor.wartoscZadana;
    liczbaRegulatorowPID++;
    stringstream strumien;
    strumien << liczbaRegulatorowPID;
    this->nazwa = "RegulatorPID" + strumien.str();
}


//======= destruktor  ==========================================================
/**
 * Destruktor klasy RegulatorPID - aktualizuje liczbe obiektow RegulatorPID.
 */
RegulatorPID::~RegulatorPID() {
    liczbaRegulatorowPID--;
}


//======= funkcja symuluj ======================================================
/**
 * Symuluje dzialanie regulatora - generuje wartosc sterowania na podstawie 
 * otrzymanej wartosci wyjscia obiektu oraz uaktualnia wartosci uchybu, 
 * sterowania, wartosci zadanej oraz wartosci regulowanej. Funkcja jest implementacja
 * funkcji abstrakcyjnej interfejsu ObiektDyskretny
 *		@param y - wyjscie z obiektu regulacji
 *		@return sterowanie
 */
double RegulatorPID::symuluj(double y) {
    double roznica = (y - wartoscRegulowana); //obliczenie roznicy y(i) - y(i-1)
    wartoscRegulowana = y;
    // ---- generacja wartosciZadanej ------------------------------------------
    wartoscZadana = 0.0;
    for (int i = 0; i < generatory.size(); i++) {
        wartoscZadana += generatory.at(i)->generuj();
    }
    // ---- obliczenie uchybu --------------------------------------------------
    uchyb = wartoscZadana - wartoscRegulowana;
    // ---- obliczenie sterowania ----------------------------------------------
    sterowanie = k * (b * wartoscZadana - wartoscRegulowana); //czescP
    sterowanie += czescI; //dodanie wartosci calki
    czescI += k / Ti*uchyb; //calkowanie
	//sterowanie += czescD; //dodanie wartosci rozniczki
    czescD = Td / (Td + N)*(czescD - k * N * roznica); //roznicowanie
    sterowanie += czescD; //dodanie wartosci rozniczki
    return sterowanie;
}

//======= funkcja zmierzWyjscie ================================================
/**
 * Funkcja probkujaca Regulator. Funkcja symuluje dzialanie czujnika pomiarowego
 * - pobiera wartosc wyjscia obiektu w kolejnej chwili czasu - przyszlej probce 
 * przy zalozeniu braku zmiany sterowania.
 *      @return przyszle wyjscie z obiektu
 */
double RegulatorPID::zmierzWyjscie() {
    return sterowanie;
}

//======= funkcja ustawParametry  =============================================
/**
 * Ustawia parametry obiektu na podstawie otrzymanej mapy. Metoda wykorzystywana
 * podczas wczytywania obiektow z pliku konfiguracyjnego. Zwracana wartosc zalezy
 * od poprawnosci mapy podanej jako argument
 *		@param mapaKonfig - mapa z konfiguracja
 *		@return <code>true</code> jezeli mapa byla poprawna mapa RegulatoraP
 *				i udalo sie ustawic parametry
 *	            <code>false</code> w przeciwnym wypadku
 */
bool RegulatorPID::ustawParametry(std::map<std::string, std::string> mapaKonfig) {
    stringstream strumien;
    strumien.exceptions(stringstream::badbit | stringstream::failbit);
    double tymczK, tymczTi, tymczTd, tymczN, tymczB;
    string tymczNazwa;
    //cout<<"wywolano ustawParametry PIDA"<<endl;

    //----- sprawdzenie wielkosci mapy -----------------------------------------
    if (mapaKonfig.size() != RegulatorPID::kluczeMapy.size()){
        //cout << "blad wielkosci mapy PID" << endl;
        throw Wyjatek("Blad w funkcji RegulatorPID::ustawParametry w obiekcie: " + this->pobierzNazwe()
            + ".\n Nie zmieniono parametrow!");
    }
    
    //cout<<"klucze!"<<endl;

    //----- sprawdzenie poprawnosci kluczy mapy --------------------------------
    for (int i = 0; i < RegulatorPID::kluczeMapy.size(); i++) {
        if (mapaKonfig[RegulatorPID::kluczeMapy.at(i)].empty()){
            //cout << "blad kluczy mapy PID" << endl;
            throw Wyjatek("Blad w funkcji RegulatorPID::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!"
                + "\n Brak klucza:" + RegulatorPID::kluczeMapy.at(i));
        }
    }
    
    //cout<<"wartosci!"<<endl;
    
    //----- przepisanie wartosci mapy do zmiennych tymczasowych ----------------
    try {
        strumien.str(mapaKonfig["[nazwa]="]);
        strumien >> tymczNazwa;
        strumien.clear();
        strumien.str(mapaKonfig["[k]="]);
        strumien >> tymczK;
        strumien.clear();
        strumien.str(mapaKonfig["[Ti]="]);
        strumien >> tymczTi;
        strumien.clear();
        strumien.str(mapaKonfig["[Td]="]);
        strumien >> tymczTd;
        strumien.clear();
        strumien.str(mapaKonfig["[N]="]);
        strumien >> tymczN;
        strumien.clear();
        strumien.str(mapaKonfig["[b]="]);
        strumien >> tymczB;
        strumien.clear();
    } catch (stringstream::failure e) { //wyjatek strumienia - nie wczytano poprawnie parametrow
        strumien.clear();
        //cout << "blad strumienia mapy PID" << endl;
        throw Wyjatek("Blad w funkcji RegulatorPID::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!");
        return false;
    }
    
    //cout<<"przepisanie!"<<endl;

    // ---- przepisanie wartosci tymczasowych do parametrow obiektu -------------
    this->nazwa = tymczNazwa;
    this->k = tymczK;
    this->Ti = tymczTi;
    this->Td = tymczTd;
    this->b = tymczB;
    this->N = tymczN;
    return true;
}

//======= funkcja pobierzParametry  =============================================
/**
 * Pobiera parametry RegulatoraPID. Parametry sa zwracane w mapie zgodnej z zapisem
 * pliku konfiguracyjnego
 *		@return mapa z konfiguracja
 */
map<string, string> RegulatorPID::pobierzParametry() const {
    map<string, string> mapaKonfig;
    stringstream strumien;
    mapaKonfig["[typ]="] = "RegulatorPID";
    mapaKonfig["[nazwa]="] = nazwa;
    strumien << this->k;
    mapaKonfig["[k]="] = strumien.str();
    strumien.str("");
    strumien << this->Td;
    mapaKonfig["[Td]="] = strumien.str();
    strumien.str("");
    strumien << this->Ti;
    mapaKonfig["[Ti]="] = strumien.str();
    strumien.str("");
    strumien << this->N;
    mapaKonfig["[N]="] = strumien.str();
    strumien.str("");
    strumien << this->b;
    mapaKonfig["[b]="] = strumien.str();
    strumien.str("");
    return mapaKonfig;
}

//======= funkcja wirtualna zerujPamiec ========================================
/**
 * Zeruje pamiec regulatora - impelementacja funkcji czysto wirtualnej interfejsu
 * ObiektDyskretny. Wywoluje metode resetGeneratorow. W wypadku impelementacji 
 * regulatora pamietajacego poprzednie stany obiektu np. PID nalezy przyslonic
 * te metode wersja zapewniajaca czyszczenie pamieci regulatora.
 */
void RegulatorPID::zerujPamiec() {
	sterowanie = 0.0;
    uchyb = 0.0;
    wartoscRegulowana = 0.0;
    wartoscZadana = 0.0;
	czescD = 0.0;
	czescI = 0.0;
    resetGeneratorow();
}

//======= funkcja wypiszInfo ===================================================
/**
 * Funkcja wypisujaca informacje o regulatorze PID. Wypisuje 
 * nazwe obiektu.
 */
void RegulatorPID::wypiszInfo() const {
}

//------------------------------------------------------------------------------
//-------------- KONIEC [definicji klasy RegulatorPID] -------------------------
//------------------------------------------------------------------------------


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ POCZATEK [definicji klasy RegulatorGPC] +++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//======= definicja skladnika statycznego liczbaRegulatorowGPC =================
/**
 * Skladnik statyczny do przechowywania aktualnej liczby obiektow klasy
 * RegulatorGPC.
 */
int RegulatorGPC::liczbaRegulatorowGPC = 0;

//======= definicja skladnika statycznego kluczeMapy ===========================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorGPC::kluczeMapy = inicjalizujKlucze();


//======= funkcja inicjalizujKlucze ============================================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorGPC::inicjalizujKlucze() {
    vector<string> wzor;
    wzor.push_back("[nazwa]=");
    wzor.push_back("[typ]=");
    wzor.push_back("[H]=");
    wzor.push_back("[L]=");
    wzor.push_back("[ro]=");
    wzor.push_back("[alfa]=");
    wzor.push_back("[dA]=");
    wzor.push_back("[dB]=");
    wzor.push_back("[k]=");
    return wzor;
}

//======= konstruktor bezargumentowy  ==========================================
/**
 * Konstruktor klasy RegulatorGPC.
 */
RegulatorGPC::RegulatorGPC() {
    H = 0;
    L = 0;
    ro = 0;
    alfa = 0;
    wspA.push_back(1.0);
    wspA.push_back(-0.8);
    wspB.push_back(1);
    wspB.push_back(0.6);
    sterowanie = 0.0;
    uchyb = 0.0;
    wartoscRegulowana = 0.0;
    wartoscZadana = 0.0;
    dA = 3;
    dB = 3;
    k = 1;
    macierzQ = Macierz(H, L, 0.0);
    pamiecU = deque<double>(dB+k, 0.0);
    pamiecY = deque<double>(dA+1, 0.0);
    ident = Identyfikacja(dA, dB, 1, k, 0.99);
    liczbaRegulatorowGPC++;
    stringstream strumien;
    strumien << liczbaRegulatorowGPC;
    this->nazwa = "RegulatorGPC" + strumien.str();
}

//======= konstruktor  =========================================================
/**
 * Konstruktor klasy RegulatorGPC.
 *      @param H - horyzont predykcji
 *      @param L - horyzont sterowania
 *      @param ro - waga na sterowania
 *      @param alfa - parametr modelu odniesienia
 *      @param dA - stopien wielomianu A
 *      @param dB - stopien wielomianu B
 *      @param k - opoznienie
 */
RegulatorGPC::RegulatorGPC(int H, int L, double ro, double alfa, int dA, int dB, int k) {
    this->H = H;
    this->L = L;
    this->ro = ro;
    this->alfa = alfa;
    this->dA = dA;
    this->dB = dB;
    this->k = k;
    wspA.push_back(1.0);
    wspA.push_back(-0.8);
    wspB.push_back(1);
    wspB.push_back(0.6);
    sterowanie = 0.0;
    uchyb = 0.0;
    wartoscRegulowana = 0.0;
    wartoscZadana = 0.0;
    macierzQ = Macierz(H, L, 0.0);
    pamiecU = deque<double>(dB+k, 0.0);
    pamiecY = deque<double>(dA+1, 0.0);
    cout<<"macierzQ"<<endl<<macierzQ<<endl;
    ident = Identyfikacja(dA, dB, 1, k, 0.99);
    liczbaRegulatorowGPC++;
    stringstream strumien;
    strumien << liczbaRegulatorowGPC;
    this->nazwa = "RegulatorGPC" + strumien.str();
}


//======= konstruktor kopiujacy ================================================
/**
 * Konstruktor kopiujacy klasy RegulatorGPC.
 *      @param &wzor - referencja do obiektu klasy RegulatorPID, 
 *                      ktory ma byc skopiowany.
 */
RegulatorGPC::RegulatorGPC(const RegulatorGPC & wzor) {
    this->H = wzor.H;
    this->L = wzor.L;
    ro = wzor.ro;
    this->alfa = wzor.alfa;
    this->dA = wzor.dA;
    this->dB = wzor.dB;
    this->k = wzor.k;
    this->generatory = wzor.generatory;
    this->sterowanie = wzor.sterowanie;
    this->uchyb = wzor.uchyb;
    this->wartoscRegulowana = wzor.wartoscRegulowana;
    this->wartoscZadana = wzor.wartoscZadana;
    this->ident = wzor.ident;
    this->macierzQ = wzor.macierzQ;
    this->pamiecU = wzor.pamiecU;
    this->pamiecY = wzor.pamiecY;
    liczbaRegulatorowGPC++;
    stringstream strumien;
    strumien << liczbaRegulatorowGPC;
    this->nazwa = "RegulatorGPC" + strumien.str();
}


//======= destruktor  ==========================================================
/**
 * Destruktor klasy RegulatorGPC - aktualizuje liczbe obiektow RegulatorGPC.
 */
RegulatorGPC::~RegulatorGPC() {
    liczbaRegulatorowGPC--;
}


//======= funkcja symuluj ======================================================
/**
 * Symuluje dzialanie regulatora - generuje wartosc sterowania na podstawie 
 * otrzymanej wartosci wyjscia obiektu oraz uaktualnia wartosci uchybu, 
 * sterowania, wartosci zadanej oraz wartosci regulowanej. Funkcja jest implementacja
 * funkcji abstrakcyjnej interfejsu ObiektDyskretny
 *	@param y - wyjscie z obiektu regulacji
 *	@return sterowanie
 */
double RegulatorGPC::symuluj(double y) {
    double staraWartReg = wartoscRegulowana;
    wartoscRegulowana = y;
    // ---- generacja wartosciZadanej ------------------------------------------
    wartoscZadana = 0.0;
    for (int i = 0; i < generatory.size(); i++) {
        wartoscZadana += generatory.at(i)->generuj();
    }
    //cout<<"wartoscZadana = " <<wartoscZadana<<endl;
    // ----- identyfikacja -----------------------------------------------------
    //cout<<"identyfikacja"<<endl;
    //cout<<"sterowania = "<<sterowanie<<endl;
    //cout<<"wartosc regulowana = "<<wartoscRegulowana<<endl;
    
    try{
        ident.krokIdentyfikacji(sterowanie, staraWartReg);
        wspA = ident.pobierzA();
        wspB = ident.pobierzB();
    }catch(Wyjatek &e){
        cout<<e.pobierzNazwe()<<endl;
        pamiecU.push_front(0.0);
        pamiecU.pop_back();
        return sterowanie;
    }
    cout<<"wspA "<<wspA<<endl;
    cout<<"wspB "<<wspB<<endl;
    
    wyznaczMacierzQ();
    try{
        //cout<<"QTQ"<<endl<<macierzQ.transponuj()*macierzQ<<endl;
        //cout<<"QtQ+roI"<<endl<<(macierzQ.transponuj()*macierzQ+ro*Macierz::I(L))<<endl;
        //cout<<"(QtQ+roI)-1"<<endl<<(macierzQ.transponuj()*macierzQ+ro*Macierz::I(L)).odwroc()<<endl;
        //cout<<"QtQ"<<endl<<(macierzQ.transponuj()*macierzQ+ro*Macierz::I(L)).odwroc()*macierzQ.transponuj()<<endl;
        wektorQ = ((macierzQ.transponuj()*macierzQ+ro*Macierz::I(L)).odwroc()*macierzQ.transponuj()).wiersz(0);
        cout<<"wektorQ"<<endl<<wektorQ<<endl;
    }catch(Wyjatek &e){
        cout<<e.pobierzNazwe()<<endl;
        cout<<"Do macierzy QTQ dodano 0.5*I"<<endl;
        //macierzQ += 1*Macierz::I(L);
        wektorQ = ((macierzQ.transponuj()*macierzQ+0.5*Macierz::I(L)).odwroc()*macierzQ.transponuj()).wiersz(0);
        cout<<"wektorQ"<<endl<<wektorQ<<endl;
    }
    
    wyznaczTrajektorieOdniesienia();
    wyznaczOdpowiedzSwobodna();
    //cout<<"wektorQ"<<endl<<wektorQ<<endl;
    //cout<<"wektorW0"<<endl<<wektorW0<<endl;
    //cout<<"wektorY0"<<endl<<wektorY0<<endl;
    //cout<<"W0 - Y0"<<endl<<wektorW0 - wektorY0<<endl;
    double deltaU = (wektorQ * (wektorW0 - wektorY0)).at(0, 0);
    //cout<<"deltaU = "<< deltaU <<endl;
    pamiecU.push_front(deltaU);
    pamiecU.pop_back();
    sterowanie += deltaU;
    return sterowanie;
}

//======= funkcja wyznaczMacierzQ ==============================================
/**
 * Wyznacza odpowiedz skokowa zidentyfikowanego obiektu B/A. Oraz wypelnia macierzQ.
 */
void RegulatorGPC::wyznaczMacierzQ() {
    //cout<<"wyznacz MacierzQ"<<endl;
    //cout<<"stara  macierzQ"<<endl<<macierzQ<<endl;
    macierzQ = Macierz(H, L, 0.0);
    cout<<"nowa  macierzQ"<<endl<<macierzQ<<endl;
    deque<double> pamU(dB+k, 0.0);
    deque<double> pamY(dA, 0.0);
    vector<double> wektorH;
    for(int i=0; i<H; i++){
        //cout<<"\ti="<<i<<endl;
        pamU.push_front(1.0);
        pamU.pop_back();
        //cout<<"\tpamiecU="<<pamU<<endl;
        double y = 0.0;
        //-------- wplyw wyjsc -------------------------------------------------
        y -= inner_product(pamY.begin(), pamY.end(), wspA.begin() + 1, 0.0);
        //-------- wplyw wejsc -------------------------------------------------
        y += inner_product(pamU.begin() + k-1, pamU.end(), wspB.begin(), 0.0);
        wektorH.push_back(y);
        pamY.push_front(y);
        pamY.pop_back();
        //cout<<"\tpamiecY="<<pamY<<endl;
        //cout<<"\ty="<<y<<endl;
        //-------- wypelnienie macierzy Q --------------------------------------
        for(int j=0; j<L; j++){
            if(j<=i) macierzQ.at(i, j) = wektorH.at(i-j);
        }
    }   
    cout<<"macierzQ"<<endl<<macierzQ<<endl;
    //cout<<"CCC"<<endl;
}

//======= funkcja wyznaczTrajektorieOdniesienia ================================
/**
 * Wyznacza trajektorie odniesienia.
 */
void RegulatorGPC::wyznaczTrajektorieOdniesienia(){
    wektorW0 = Macierz(H, 1, 0.0);
    double w0pop = wartoscRegulowana;
    wektorW0.at(0, 0) = (wartoscRegulowana);
    //cout<<"y = "<<wartoscRegulowana<<endl;
    double w = wartoscZadana;
    //cout<<"w = "<<w<<endl;
    for(int i=0; i<H; i++){
        //wektorW0.at(i, 0) = ((1-alfa)*w+alfa*wektorW0.at(i-1, 0));
        wektorW0.at(i, 0) = ((1-alfa)*w+alfa*w0pop);
        w0pop = wektorW0.at(i, 0);
    }
    //cout<<"wektorW0"<<endl<<wektorW0<<endl;
}

//======= funkcja wyznaczOdpowiedzSwobodna =====================================
/**
 * Wyznacza odpowiedz swobodna.
 */
void RegulatorGPC::wyznaczOdpowiedzSwobodna(){
    wektorY0 = Macierz(H, 1, 0.0);
    pamiecY.push_front(wartoscRegulowana);
    pamiecY.pop_back();
    //cout<<"pamiecY: " << pamiecY<<endl;
    //cout<<"pamiecU: " << pamiecU<<endl;
    vector<double> noweWspA(dA+2, 0.0);
    deque<double> pamU = pamiecU;
    deque<double> pamY = pamiecY;
    adjacent_difference(wspA.rbegin(), wspA.rend(), noweWspA.rbegin());
    noweWspA.at(0) = 1;
    //cout<<"Nowe wsp A: "<<noweWspA<<endl;
     for(int i=0; i<H; i++){
        //cout<<"\ti="<<i<<endl;
        pamU.push_front(0.0);
        pamU.pop_back();
        //cout<<"\tpamU="<<pamU<<endl;
        //cout<<"\tpamY="<<pamY<<endl;
        double y = 0.0;
        //-------- wplyw wyjsc -------------------------------------------------
        y += inner_product(pamY.begin(), pamY.end(), noweWspA.begin() + 1, 0.0);
        //-------- wplyw wejsc -------------------------------------------------
        y += inner_product(pamU.begin() + k-1, pamU.end(), wspB.begin(), 0.0);
        wektorY0.at(i, 0) = y;
        pamY.push_front(y);
        pamY.pop_back();
        //cout<<"\ty="<<y<<endl;
    }   
    //cout<<"wektorY0"<<endl<<wektorY0<<endl;
}


//======= funkcja zmierzWyjscie ================================================
/**
 * Funkcja probkujaca Regulator. Funkcja symuluje dzialanie czujnika pomiarowego
 * - pobiera wartosc wyjscia obiektu w kolejnej chwili czasu - przyszlej probce 
 * przy zalozeniu braku zmiany sterowania.
 *      @return przyszle wyjscie z obiektu
 */
double RegulatorGPC::zmierzWyjscie() {
    return sterowanie;
}

//======= funkcja pobierzWspA ==================================================
/**
 * Funkcja pobierajaca zidentyfikowana parametry wielomianu A.
 *      @return wektor wspolczynnikow wielomianu A
 */
vector<double> RegulatorGPC::pobierzWspA() const{
    return wspA;
}

//======= funkcja pobierzWspB ==================================================
/**
 * Funkcja pobierajaca zidentyfikowana parametry wielomianu B.
 *      @return wektor wspolczynnikow wielomianu B
 */
vector<double> RegulatorGPC::pobierzWspB() const{
    return wspB;
}

//======= funkcja ustawH  ======================================================
/**
 * Ustawia horyzont predykcji H.
 *      @param H - nowy horyzont predykcji
 */
void RegulatorGPC::ustawH(int H) {
    this->H = H;
}

//======= funkcja ustawL  ======================================================
/**
 * Ustawia horyzont sterowania L.
 *      @param L - nowy horyzont sterowania
 */
void RegulatorGPC::ustawL(int L) {
    this->L = L;
}

//======= funkcja ustawAlfa ====================================================
/**
 * Ustawia parametr Alfa.
 *      @param alfa - nowy horyzont sterowania
 */
void RegulatorGPC::ustawAlfa(double alfa) {
    this->alfa = alfa;
}

//======= funkcja ustawRo ====================================================
/**
 * Ustawia parametr ro.
 *      @param ro - nowy horyzont sterowania
 */
void RegulatorGPC::ustawRo(double ro) {
    this->ro = ro;
}

//======= funkcja ustawParametry  =============================================
/**
 * Ustawia parametry obiektu na podstawie otrzymanej mapy. Metoda wykorzystywana
 * podczas wczytywania obiektow z pliku konfiguracyjnego. Zwracana wartosc zalezy
 * od poprawnosci mapy podanej jako argument
 *		@param mapaKonfig - mapa z konfiguracja
 *		@return <code>true</code> jezeli mapa byla poprawna mapa RegulatoraP
 *				i udalo sie ustawic parametry
 *	            <code>false</code> w przeciwnym wypadku
 */
bool RegulatorGPC::ustawParametry(std::map<std::string, std::string> mapaKonfig) {
    stringstream strumien;
    strumien.exceptions(stringstream::badbit | stringstream::failbit);
    double tymczRo, tymczAlfa;
    int tymczH, tymczL, tymczK, tymczDA, tymczDB;
    string tymczNazwa;
    cout<<"wywolano ustawParametry GPC"<<endl;

    //----- sprawdzenie wielkosci mapy -----------------------------------------
    if (mapaKonfig.size() != RegulatorGPC::kluczeMapy.size()){
        //cout << "blad wielkosci mapy GPC" << endl;
        throw Wyjatek("Blad w funkcji RegulatorGPC::ustawParametry w obiekcie: " + this->pobierzNazwe()
            + ".\n Nie zmieniono parametrow!");
    }
    
    //cout<<"klucze!"<<endl;

    //----- sprawdzenie poprawnosci kluczy mapy --------------------------------
    for (int i = 0; i < RegulatorGPC::kluczeMapy.size(); i++) {
        if (mapaKonfig[RegulatorGPC::kluczeMapy.at(i)].empty()){
            //cout << "blad kluczy mapy PID" << endl;
            throw Wyjatek("Blad w funkcji RegulatorGPC::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!"
                + "\n Brak klucza:" + RegulatorGPC::kluczeMapy.at(i));
        }
    }
    
    //cout<<"wartosci!"<<endl;
    
    //----- przepisanie wartosci mapy do zmiennych tymczasowych ----------------
    try {
        strumien.str(mapaKonfig["[nazwa]="]);
        strumien >> tymczNazwa; cout<<tymczNazwa;
        strumien.clear();
        strumien.str(mapaKonfig["[H]="]);
        strumien >> tymczH; cout<<tymczH;
        strumien.clear();
        strumien.str(mapaKonfig["[L]="]);
        strumien >> tymczL; cout<<tymczL;
        strumien.clear();
        strumien.str(mapaKonfig["[ro]="]);
        strumien >> tymczRo; cout<<tymczRo;
        strumien.clear();
        strumien.str(mapaKonfig["[alfa]="]);
        strumien >> tymczAlfa; cout<<tymczAlfa;
        strumien.clear();
        strumien.str(mapaKonfig["[dA]="]);
        strumien >> tymczDA; cout<<tymczDA;
        strumien.clear();
        strumien.str(mapaKonfig["[dB]="]);
        strumien >> tymczDB; cout<<tymczDB;
        strumien.clear();
        strumien.str(mapaKonfig["[k]="]);
        strumien >> tymczK; cout<<tymczK;
        strumien.clear();
    } catch (stringstream::failure e) { //wyjatek strumienia - nie wczytano poprawnie parametrow
        strumien.clear();
        //cout << "blad strumienia mapy PID" << endl;
        throw Wyjatek("Blad w funkcji RegulatorGPC::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!");
        return false;
    }
    
    //cout<<"przepisanie!"<<endl;

    // ---- przepisanie wartosci tymczasowych do parametrow obiektu -------------
    this->nazwa = tymczNazwa;
    this->H = tymczH;
    this->L = tymczL;
    this->ro = tymczRo;
    this->alfa = tymczAlfa;
    this->dA = tymczDA;
    this->dB = tymczDB;
    this->k = tymczK;
    macierzQ = Macierz(H, L, 0.0);
    pamiecU = deque<double>(dB+k, 0.0);
    pamiecY = deque<double>(dA+1, 0.0);
    //cout<<"macierzQ"<<endl<<macierzQ<<endl;
    ident = Identyfikacja(dA, dB, 1, k, 0.99);
    return true;
}

//======= funkcja pobierzParametry  =============================================
/**
 * Pobiera parametry RegulatoraGPC. Parametry sa zwracane w mapie zgodnej z zapisem
 * pliku konfiguracyjnego
 *		@return mapa z konfiguracja
 */
map<string, string> RegulatorGPC::pobierzParametry() const {
    map<string, string> mapaKonfig;
    stringstream strumien;
    mapaKonfig["[typ]="] = "RegulatorGPC";
    mapaKonfig["[nazwa]="] = nazwa;
    strumien << this->H;
    mapaKonfig["[H]="] = strumien.str();
    strumien.str("");
    strumien << this->L;
    mapaKonfig["[L]="] = strumien.str();
    strumien.str("");
    strumien << this->ro;
    mapaKonfig["[ro]="] = strumien.str();
    strumien.str("");
    strumien << this->alfa;
    mapaKonfig["[alfa]="] = strumien.str();
    strumien.str("");
    strumien << this->dA;
    mapaKonfig["[dA]="] = strumien.str();
    strumien.str("");
    strumien << this->dB;
    mapaKonfig["[dB]="] = strumien.str();
    strumien.str("");
    strumien << this->k;
    mapaKonfig["[k]="] = strumien.str();
    strumien.str("");
    return mapaKonfig;
}

//======= funkcja wirtualna zerujPamiec ========================================
/**
 * Zeruje pamiec regulatora - impelementacja funkcji czysto wirtualnej interfejsu
 * ObiektDyskretny. Wywoluje metode resetGeneratorow. W wypadku impelementacji 
 * regulatora pamietajacego poprzednie stany obiektu np. PID nalezy przyslonic
 * te metode wersja zapewniajaca czyszczenie pamieci regulatora.
 */
void RegulatorGPC::zerujPamiec() {
    resetGeneratorow();
	macierzQ = Macierz(H, L, 0.0);
    pamiecU = deque<double>(dB+k, 0.0);
    pamiecY = deque<double>(dA+1, 0.0);
    //cout<<"macierzQ"<<endl<<macierzQ<<endl;
    ident = Identyfikacja(dA, dB, 1, k, 0.99);
	sterowanie = 0.0;
    uchyb = 0.0;
    wartoscRegulowana = 0.0;
    wartoscZadana = 0.0;
}

//======= funkcja wypiszInfo ===================================================
/**
 * Funkcja wypisujaca informacje o regulatorze PID. Wypisuje 
 * nazwe obiektu.
 */
void RegulatorGPC::wypiszInfo() const {
    cout<<nazwa<<endl;
    cout<<"\tH="<<H<<endl;
    cout<<"\tL="<<L<<endl;
    cout<<"\talfa="<<alfa<<endl;
    cout<<"\tro="<<ro<<endl;
    cout<<"\tdA="<<dA<<endl;
    cout<<"\tdB="<<dB<<endl;
    cout<<"\tk="<<k<<endl;
}
//------------------------------------------------------------------------------
//-------------- KONIEC [definicji klasy RegulatorGPC] -------------------------
//------------------------------------------------------------------------------


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ POCZATEK [definicji klasy PrzekaznikH] +++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//======= definicja skladnika statycznego liczbaPrzekaznikowH =================
/**
 * Skladnik statyczny do przechowywania aktualnej liczby obiektow klasy
 * PrzekaznikH.
 */
int PrzekaznikH::liczbaPrzekaznikowH = 0;

//======= definicja skladnika statycznego kluczeMapy ===========================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> PrzekaznikH::kluczeMapy = inicjalizujKlucze();


//======= funkcja inicjalizujKlucze ============================================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> PrzekaznikH::inicjalizujKlucze() {
    vector<string> wzor;
    wzor.push_back("[nazwa]=");
    wzor.push_back("[d]=");
    wzor.push_back("[typ]=");
    wzor.push_back("[eps]=");
    return wzor;
}


//======= konstruktor  =========================================================
/**
 * Konstruktor klasy PrzekaznikH.
 *      @param d - wzmocnienie regulatora
 */
PrzekaznikH::PrzekaznikH(double d) {
    this->d = d;
    this->eps = 1;
	this->flagaPrzekaznikPositive = true;
    this->sterowanie = 0.0;
    this->uchyb = 0.0;
    this->wartoscRegulowana = 0.0;
    this->wartoscZadana = 0.0;
    liczbaPrzekaznikowH++;
    stringstream strumien;
    strumien << liczbaPrzekaznikowH;
    this->nazwa = "PrzekaznikH" + strumien.str();
	
	Generator* generator = new GeneratorSkoku(0, 1);		
	this->dodajGenerator(generator);
}


//======= konstruktor kopiujacy ================================================
/**
 * Konstruktor kopiujacy klasy PrzekaznikH.
 *      @param &wzor - referencja do obiektu klasy PrzekaznikH, 
 *                      ktory ma byc skopiowany.
 */
PrzekaznikH::PrzekaznikH(const PrzekaznikH & wzor) {
    this->d = wzor.d;
    this->eps= wzor.eps;
    this->generatory = wzor.generatory;
    this->sterowanie = wzor.sterowanie;
    this->uchyb = wzor.uchyb;
    this->wartoscRegulowana = wzor.wartoscRegulowana;
    this->wartoscZadana = wzor.wartoscZadana;
    liczbaPrzekaznikowH++;
    stringstream strumien;
    strumien << liczbaPrzekaznikowH;
    this->nazwa = "PrzekaznikH" + strumien.str();
}


//======= destruktor  ==========================================================
/**
 * Destruktor klasy PrzekaznikH - aktualizuje liczbe obiektow RegulatorPID.
 */
PrzekaznikH::~PrzekaznikH() {
    liczbaPrzekaznikowH--;
}


//======= funkcja symuluj ======================================================
/**
 * Symuluje dzialanie regulatora - generuje wartosc sterowania na podstawie 
 * otrzymanej wartosci wyjscia obiektu oraz uaktualnia wartosci uchybu, 
 * sterowania, wartosci zadanej oraz wartosci regulowanej. Funkcja jest implementacja
 * funkcji abstrakcyjnej interfejsu ObiektDyskretny
 *		@param y - wyjscie z obiektu regulacji
 *		@return sterowanie
 */
double PrzekaznikH::symuluj(double y) {
    double roznica = (y - wartoscRegulowana); //obliczenie roznicy y(i) - y(i-1)
    wartoscRegulowana = y;
    // ---- generacja wartosciZadanej ------------------------------------------
    wartoscZadana = 0.0;
    for (int i = 0; i < generatory.size(); i++) {
        wartoscZadana += generatory.at(i)->generuj();
    }
    // ---- obliczenie uchybu --------------------------------------------------
    uchyb = wartoscZadana - wartoscRegulowana;

    // ---- obliczenie sterowania ----------------------------------------------
    if(uchyb < eps && flagaPrzekaznikPositive == false)
		sterowanie = -d;
	else if(uchyb >= eps && flagaPrzekaznikPositive == false) {
		sterowanie = d;
		flagaPrzekaznikPositive = true;
		}
	else if(uchyb >= -eps && flagaPrzekaznikPositive == true)
		sterowanie = d;
	else if(uchyb < -eps && flagaPrzekaznikPositive == true) {
		sterowanie = -d;
		flagaPrzekaznikPositive = false;
		}
	else exit(0);

    return sterowanie;
}

//======= funkcja zmierzWyjscie ================================================
/**
 * Funkcja probkujaca Regulator. Funkcja symuluje dzialanie czujnika pomiarowego
 * - pobiera wartosc wyjscia obiektu w kolejnej chwili czasu - przyszlej probce 
 * przy zalozeniu braku zmiany sterowania.
 *      @return przyszle wyjscie z obiektu
 */
double PrzekaznikH::zmierzWyjscie() {
    return sterowanie;
}

//======= funkcja ustawParametry  =============================================
/**
 * Ustawia parametry obiektu na podstawie otrzymanej mapy. Metoda wykorzystywana
 * podczas wczytywania obiektow z pliku konfiguracyjnego. Zwracana wartosc zalezy
 * od poprawnosci mapy podanej jako argument
 *		@param mapaKonfig - mapa z konfiguracja
 *		@return <code>true</code> jezeli mapa byla poprawna mapa RegulatoraP
 *				i udalo sie ustawic parametry
 *	            <code>false</code> w przeciwnym wypadku
 */
bool PrzekaznikH::ustawParametry(std::map<std::string, std::string> mapaKonfig) {
    stringstream strumien;
    strumien.exceptions(stringstream::badbit | stringstream::failbit);
    double tymczD, tymczE;
    string tymczNazwa;
    //cout<<"wywolano ustawParametry PIDA"<<endl;

    //----- sprawdzenie wielkosci mapy -----------------------------------------
    if (mapaKonfig.size() != PrzekaznikH::kluczeMapy.size()){
        //cout << "blad wielkosci mapy PrzekaznikH" << endl;
        throw Wyjatek("Blad w funkcji PrzekaznikH::ustawParametry w obiekcie: " + this->pobierzNazwe()
            + ".\n Nie zmieniono parametrow!");
    }
    
    //cout<<"klucze!"<<endl;

    //----- sprawdzenie poprawnosci kluczy mapy --------------------------------
    for (int i = 0; i < PrzekaznikH::kluczeMapy.size(); i++) {
		if (mapaKonfig[PrzekaznikH::kluczeMapy.at(i)].empty()){
            //cout << "blad kluczy mapy PrzekaznikH" << endl;
            throw Wyjatek("Blad w funkcji PrzekaznikH::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!"
                + "\n Brak klucza:" + PrzekaznikH::kluczeMapy.at(i));
        }
    }
    
    //cout<<"wartosci!"<<endl;
    
    //----- przepisanie wartosci mapy do zmiennych tymczasowych ----------------
    try {
        strumien.str(mapaKonfig["[nazwa]="]);
        strumien >> tymczNazwa;
        strumien.clear();
        strumien.str(mapaKonfig["[d]="]);
        strumien >> tymczD;
        strumien.clear();
        strumien.str(mapaKonfig["[eps]="]);
        strumien >> tymczE;
        strumien.clear();
        strumien.clear();
    } catch (stringstream::failure e) { //wyjatek strumienia - nie wczytano poprawnie parametrow
        strumien.clear();
        //cout << "blad strumienia mapy PrzekaznikH" << endl;
        throw Wyjatek("Blad w funkcji PrzekaznikH::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!");
        return false;
    }
    
    //cout<<"przepisanie!"<<endl;

    // ---- przepisanie wartosci tymczasowych do parametrow obiektu -------------
    this->nazwa = tymczNazwa;
    this->d = tymczD;
    this->eps = tymczE;
    return true;
}

//======= funkcja pobierzParametry  =============================================
/**
 * Pobiera parametry PrzekaznikH. Parametry sa zwracane w mapie zgodnej z zapisem
 * pliku konfiguracyjnego
 *		@return mapa z konfiguracja
 */
map<string, string> PrzekaznikH::pobierzParametry() const {
    map<string, string> mapaKonfig;
    stringstream strumien;
    mapaKonfig["[typ]="] = "PrzekaznikH";
    mapaKonfig["[nazwa]="] = nazwa;
    strumien << this->d;
    mapaKonfig["[d]="] = strumien.str();
    strumien.str("");
    strumien << this->eps;
    mapaKonfig["[eps]="] = strumien.str();
    strumien.str("");
    return mapaKonfig;
}

//======= funkcja wirtualna zerujPamiec ========================================
/**
 * Zeruje pamiec regulatora - impelementacja funkcji czysto wirtualnej interfejsu
 * ObiektDyskretny. Wywoluje metode resetGeneratorow. W wypadku impelementacji 
 * regulatora pamietajacego poprzednie stany obiektu np. PID nalezy przyslonic
 * te metode wersja zapewniajaca czyszczenie pamieci regulatora.
 */
void PrzekaznikH::zerujPamiec() {
	sterowanie = 0.0;
    uchyb = 0.0;
    wartoscRegulowana = 0.0;
    wartoscZadana = 0.0;
    resetGeneratorow();
}

//======= funkcja wypiszInfo ===================================================
/**
 * Funkcja wypisujaca informacje o regulatorze PrzekaznikH. Wypisuje 
 * nazwe obiektu.
 */
void PrzekaznikH::wypiszInfo() const {
}

//------------------------------------------------------------------------------
//-------------- KONIEC [definicji klasy PrzekaznikH] -------------------------
//------------------------------------------------------------------------------



//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ POCZATEK [definicji klasy RegulatorPIDAdapt] +++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//======= definicja skladnika statycznego liczbaRegulatorowPIDAdapt =================
/**
 * Skladnik statyczny do przechowywania aktualnej liczby obiektow klasy
 * RegulatorPIDAdapt.
 */
int RegulatorPIDAdapt::liczbaRegulatorowPIDAdapt = 0;

//======= definicja skladnika statycznego kluczeMapy ===========================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorPIDAdapt::kluczeMapy = inicjalizujKlucze();


//======= funkcja inicjalizujKlucze ============================================
/**
 * Skladnik statyczny do przechowywania wzoru kluczy mapy konfiguracyjnej.
 */
vector<string> RegulatorPIDAdapt::inicjalizujKlucze() {
    vector<string> wzor;
    wzor.push_back("[nazwa]=");
    wzor.push_back("[k]=");
    wzor.push_back("[typ]=");
    wzor.push_back("[Ti]=");
    wzor.push_back("[Td]=");
    wzor.push_back("[N]=");
    wzor.push_back("[b]=");
    return wzor;
}


//======= konstruktor  =========================================================
/**
 * Konstruktor klasy RegulatorPIDAdapt.
 *      @param k - wzmocnienie regulatora
 */
RegulatorPIDAdapt::RegulatorPIDAdapt(double k)
: przekaznikExp(0.3) {
    this->k = k;
    this->Td = 0;
    this->Ti = 100000;
    this->N = 1;
    this->czescI = 0.0;
    this->czescD = 0.0;
    this->sterowanie = 0.0;
    this->uchyb = 0.0;
    this->wartoscRegulowana = 0.0;
    this->wartoscZadana = 0.0;
	this->flagaSamonastrajania = false;
	this->flagaKoniecSamonastrajania = false;
    liczbaRegulatorowPIDAdapt++;
    stringstream strumien;
    strumien << liczbaRegulatorowPIDAdapt;
    this->nazwa = "RegulatorPIDAdapt" + strumien.str();
}


//======= konstruktor kopiujacy ================================================
/**
 * Konstruktor kopiujacy klasy RegulatorPIDAdapt.
 *      @param &wzor - referencja do obiektu klasy RegulatorPIDAdapt, 
 *                      ktory ma byc skopiowany.
 */
RegulatorPIDAdapt::RegulatorPIDAdapt(const RegulatorPIDAdapt & wzor)
: przekaznikExp(wzor.przekaznikExp) {
    this->k = wzor.k;
    this->Td = wzor.Td;
    this->Ti = wzor.Ti;
    this->N = wzor.N;
    this->czescI = wzor.czescI;
    this->czescD = wzor.czescD;
    this->generatory = wzor.generatory;
    this->sterowanie = wzor.sterowanie;
    this->uchyb = wzor.uchyb;
    this->wartoscRegulowana = wzor.wartoscRegulowana;
    this->wartoscZadana = wzor.wartoscZadana;
	this->flagaSamonastrajania = wzor.flagaSamonastrajania;
	this->flagaKoniecSamonastrajania = wzor.flagaKoniecSamonastrajania;
    liczbaRegulatorowPIDAdapt++;
    stringstream strumien;
    strumien << liczbaRegulatorowPIDAdapt;
    this->nazwa = "RegulatorPIDAdapt" + strumien.str();
}


//======= destruktor  ==========================================================
/**
 * Destruktor klasy RegulatorPID - aktualizuje liczbe obiektow RegulatorPID.
 */
RegulatorPIDAdapt::~RegulatorPIDAdapt() {
    liczbaRegulatorowPIDAdapt--;
}


//======= samonastrajanie  ==========================================================
/**
 * Samonastrajanie dla RegulatorPIDAdapt.
 */
void RegulatorPIDAdapt::samonastrajanie() {
	//flagaSamonastrajania = true;
	przekaznikExp.eps = 2;
	int maxIloscWywolan = 300;
	static double kr = 1;
	static double tkr = 1;
	static bool samonastrajanieSkonczone = false;
	static bool flagaSterowaniaMax = false;
	static bool FlagaDrganiaGasnace = true;
	static bool flagaPrzedPierwszymMax = true;
	static bool flagaSpadek = false;
	static double sterowanieMax = 0;
	static double poprzednieSterowanieMax = 0;
	static double poprzedniCzas = 0;
	static double obecnyCzas = 0;
	static double poprzednieSterowanie = 0;
	static double obecneSterowanie = 0;

	/* TMP TMP TMP */
	static int tempIloscWywolan = 0;
	tempIloscWywolan++;
	
	flagaSterowaniaMax = false;

		
	if(flagaPrzedPierwszymMax == true){
		poprzednieSterowanie = sterowanie;
		poprzedniCzas = obecnyCzas;
		obecneSterowanie = sterowanie;
		obecnyCzas = tempIloscWywolan;
		if(obecneSterowanie > poprzednieSterowanie)
			flagaSpadek = false;
		else
			flagaSpadek = true;
		if((obecneSterowanie > poprzednieSterowanie && flagaSpadek == true) || (obecneSterowanie < poprzednieSterowanie && flagaSpadek == false))
			flagaPrzedPierwszymMax = false;
	}

	if(sterowanie > sterowanieMax && flagaPrzedPierwszymMax == false) {
		poprzednieSterowanieMax = sterowanieMax;
		poprzedniCzas = obecnyCzas;
		sterowanieMax = sterowanie;
		obecnyCzas = tempIloscWywolan;

		flagaSterowaniaMax = true;

		if(sterowanieMax < poprzednieSterowanieMax)
			FlagaDrganiaGasnace = true;
		else if  (sterowanieMax >= poprzednieSterowanieMax){
			FlagaDrganiaGasnace = false;
			tkr = obecnyCzas - poprzedniCzas;
			kr = przekaznikExp.d;
		}
	}

	if(flagaSterowaniaMax == true && FlagaDrganiaGasnace == true)
		przekaznikExp.d = przekaznikExp.d + 0.5;


	if( tempIloscWywolan > maxIloscWywolan || FlagaDrganiaGasnace == false)
		samonastrajanieSkonczone = true;

	if ( samonastrajanieSkonczone == true ) /* tempIloscWywolan > maxIloscWywolan */
	{
		flagaSamonastrajania = false;
		flagaKoniecSamonastrajania = true;
		tempIloscWywolan = 0;
		samonastrajanieSkonczone = false;

		//zmiana parametrow regulatora PID wedlug nastaw Zieglera Nicholsa
		k = 0.6*kr;
		Ti = 0.5*tkr;
		Td = 0.12*tkr;
	}
	/* TMP TMP TMP */
}


//======= funkcja symuluj ======================================================
/**
 * Symuluje dzialanie regulatora - generuje wartosc sterowania na podstawie 
 * otrzymanej wartosci wyjscia obiektu oraz uaktualnia wartosci uchybu, 
 * sterowania, wartosci zadanej oraz wartosci regulowanej. Funkcja jest implementacja
 * funkcji abstrakcyjnej interfejsu ObiektDyskretny
 *		@param y - wyjscie z obiektu regulacji
 *		@return sterowanie
 */
double RegulatorPIDAdapt::symuluj(double y) {
    double roznica = (y - wartoscRegulowana); //obliczenie roznicy y(i) - y(i-1)
    wartoscRegulowana = y;
    // ---- generacja wartosciZadanej ------------------------------------------
    wartoscZadana = 0.0;
    for (int i = 0; i < generatory.size(); i++) {
        wartoscZadana += generatory.at(i)->generuj();
    }
    // ---- obliczenie uchybu --------------------------------------------------
    uchyb = wartoscZadana - wartoscRegulowana;
    // ---- obliczenie sterowania ----------------------------------------------
    sterowanie = k * (b * wartoscZadana - wartoscRegulowana); //czescP
    sterowanie += czescI; //dodanie wartosci calki
    czescI += k / Ti*uchyb; //calkowanie
	//sterowanie += czescD; //dodanie wartosci rozniczki
    czescD = Td / (Td + N)*(czescD - k * N * roznica); //roznicowanie
    sterowanie += czescD; //dodanie wartosci rozniczki
    return sterowanie;
}

//======= funkcja zmierzWyjscie ================================================
/**
 * Funkcja probkujaca Regulator. Funkcja symuluje dzialanie czujnika pomiarowego
 * - pobiera wartosc wyjscia obiektu w kolejnej chwili czasu - przyszlej probce 
 * przy zalozeniu braku zmiany sterowania.
 *      @return przyszle wyjscie z obiektu
 */
double RegulatorPIDAdapt::zmierzWyjscie() {
    return sterowanie;
}

//======= funkcja ustawParametry  =============================================
/**
 * Ustawia parametry obiektu na podstawie otrzymanej mapy. Metoda wykorzystywana
 * podczas wczytywania obiektow z pliku konfiguracyjnego. Zwracana wartosc zalezy
 * od poprawnosci mapy podanej jako argument
 *		@param mapaKonfig - mapa z konfiguracja
 *		@return <code>true</code> jezeli mapa byla poprawna mapa RegulatoraP
 *				i udalo sie ustawic parametry
 *	            <code>false</code> w przeciwnym wypadku
 */
bool RegulatorPIDAdapt::ustawParametry(std::map<std::string, std::string> mapaKonfig) {
    stringstream strumien;
    strumien.exceptions(stringstream::badbit | stringstream::failbit);
    double tymczK, tymczTi, tymczTd, tymczN, tymczB;
    string tymczNazwa;
    //cout<<"wywolano ustawParametry PIDA"<<endl;

    //----- sprawdzenie wielkosci mapy -----------------------------------------
    if (mapaKonfig.size() != RegulatorPIDAdapt::kluczeMapy.size()){
        //cout << "blad wielkosci mapy PID" << endl;
        throw Wyjatek("Blad w funkcji RegulatorPIDAdapt::ustawParametry w obiekcie: " + this->pobierzNazwe()
            + ".\n Nie zmieniono parametrow!");
    }
    
    //cout<<"klucze!"<<endl;

    //----- sprawdzenie poprawnosci kluczy mapy --------------------------------
    for (int i = 0; i < RegulatorPIDAdapt::kluczeMapy.size(); i++) {
        if (mapaKonfig[RegulatorPIDAdapt::kluczeMapy.at(i)].empty()){
            //cout << "blad kluczy mapy PID" << endl;
            throw Wyjatek("Blad w funkcji RegulatorPIDAdapt::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!"
                + "\n Brak klucza:" + RegulatorPIDAdapt::kluczeMapy.at(i));
        }
    }
    
    //cout<<"wartosci!"<<endl;
    
    //----- przepisanie wartosci mapy do zmiennych tymczasowych ----------------
    try {
        strumien.str(mapaKonfig["[nazwa]="]);
        strumien >> tymczNazwa;
        strumien.clear();
        strumien.str(mapaKonfig["[k]="]);
        strumien >> tymczK;
        strumien.clear();
        strumien.str(mapaKonfig["[Ti]="]);
        strumien >> tymczTi;
        strumien.clear();
        strumien.str(mapaKonfig["[Td]="]);
        strumien >> tymczTd;
        strumien.clear();
        strumien.str(mapaKonfig["[N]="]);
        strumien >> tymczN;
        strumien.clear();
        strumien.str(mapaKonfig["[b]="]);
        strumien >> tymczB;
        strumien.clear();
    } catch (stringstream::failure e) { //wyjatek strumienia - nie wczytano poprawnie parametrow
        strumien.clear();
        //cout << "blad strumienia mapy PID" << endl;
        throw Wyjatek("Blad w funkcji RegulatorPID::ustawParametry w obiekcie: " + this->pobierzNazwe()
                + ".\n Nie zmieniono parametrow!");
        return false;
    }
    
    //cout<<"przepisanie!"<<endl;

    // ---- przepisanie wartosci tymczasowych do parametrow obiektu -------------
    this->nazwa = tymczNazwa;
    this->k = tymczK;
    this->Ti = tymczTi;
    this->Td = tymczTd;
    this->b = tymczB;
    this->N = tymczN;
    return true;
}

//======= funkcja pobierzParametry  =============================================
/**
 * Pobiera parametry RegulatorPIDAdapt. Parametry sa zwracane w mapie zgodnej z zapisem
 * pliku konfiguracyjnego
 *		@return mapa z konfiguracja
 */
map<string, string> RegulatorPIDAdapt::pobierzParametry() const {
    map<string, string> mapaKonfig;
    stringstream strumien;
    mapaKonfig["[typ]="] = "RegulatorPIDAdapt";
    mapaKonfig["[nazwa]="] = nazwa;
    strumien << this->k;
    mapaKonfig["[k]="] = strumien.str();
    strumien.str("");
    strumien << this->Td;
    mapaKonfig["[Td]="] = strumien.str();
    strumien.str("");
    strumien << this->Ti;
    mapaKonfig["[Ti]="] = strumien.str();
    strumien.str("");
    strumien << this->N;
    mapaKonfig["[N]="] = strumien.str();
    strumien.str("");
    strumien << this->b;
    mapaKonfig["[b]="] = strumien.str();
    strumien.str("");
    return mapaKonfig;
}

//======= funkcja wirtualna zerujPamiec ========================================
/**
 * Zeruje pamiec regulatora - impelementacja funkcji czysto wirtualnej interfejsu
 * ObiektDyskretny. Wywoluje metode resetGeneratorow. W wypadku impelementacji 
 * regulatora pamietajacego poprzednie stany obiektu np. PID nalezy przyslonic
 * te metode wersja zapewniajaca czyszczenie pamieci regulatora.
 */
void RegulatorPIDAdapt::zerujPamiec() {
	sterowanie = 0.0;
    uchyb = 0.0;
    wartoscRegulowana = 0.0;
    wartoscZadana = 0.0;
	czescD = 0.0;
	czescI = 0.0;
    resetGeneratorow();
}

//======= funkcja wypiszInfo ===================================================
/**
 * Funkcja wypisujaca informacje o regulatorze PID. Wypisuje 
 * nazwe obiektu.
 */
void RegulatorPIDAdapt::wypiszInfo() const {
}

PrzekaznikH* RegulatorPIDAdapt::pobierzPrzekaznikH()
{
	return &przekaznikExp;
}

bool RegulatorPIDAdapt::czySamonastrajanie() const
{
	return flagaSamonastrajania;
}

void RegulatorPIDAdapt::ustawSamonastrajanie(bool wartosc)
{
	flagaSamonastrajania = wartosc;
}

bool RegulatorPIDAdapt::czyKoniecSamonastrajania()
{
	if (flagaKoniecSamonastrajania)
	{
		flagaKoniecSamonastrajania = false;
		return true;
	}

	return false;
}

//------------------------------------------------------------------------------
//-------------- KONIEC [definicji klasy RegulatorPIDAdapt] -------------------------
//------------------------------------------------------------------------------


